#include "ros/ros.h"
#include "roboard_drivers/adc_val.h"
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include <stdlib.h>
#include <math.h>

using namespace roboard_drivers;

/*
	Helping node for calibration
		-> Do an average over 10 measures of each IR sensor to have more stable values

*/

// Values for corresponding distance (in cm) : 0 5 10 15 20 25 30

int sensor_volt0 = 0;
int sensor_volt1 = 0;
int sensor_volt2 = 0;
int sensor_volt3 = 0;
int sensor_volt4 = 0;
int sensor_volt5 = 0;

int count = 0;

void sensorsReadings(const adc_val::ConstPtr& msg)
{
	sensor_volt0 += msg->val0;
	sensor_volt1 += msg->val1;
	sensor_volt2 += msg->val2;
	sensor_volt3 += msg->val3;
	sensor_volt4 += msg->val4;
	sensor_volt5 += msg->val5;

	count++;

}

int main(int argc, char **argv)
{

	ros::init(argc, argv, "SensorsCalibration");
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe("roboard/adc", 1, sensorsReadings);
	ros::Rate loop_rate(10);
	printf("Start");
	while (ros::ok())
	{
		ros::spinOnce();

		if(count == 10) {
			printf("Sensor0 : %d, Sensor 1 : %d, Sensor 2 : %d, Sensor 3 : %d, Sensor 4 : %d, Sensor 5 : %d\n", sensor_volt0/10, 				sensor_volt1/10, sensor_volt2/10, sensor_volt3/10, sensor_volt4/10, sensor_volt5/10);

			sensor_volt0 = 0;
			sensor_volt1 = 0;
			sensor_volt2 = 0;
			sensor_volt3 = 0;
			sensor_volt4 = 0;
			sensor_volt5 = 0;
			count = 0;
		}
		loop_rate.sleep();
	}
	return 0;
}
